#include "ModuleTracking.h"

void ModuleTracking::init(QCVModuleHandlerInterface * parent)
{
	this->parent=parent;
	win=new QWidget();
  layout=new QHBoxLayout(win);
	QVBoxLayout * sub_layout=new QVBoxLayout();

	l_flann=new QLabel("<b>Flann Parametres:</b>",win);
	l_kalman=new QLabel("<b>Tracking Parametres:</b>",win);
	l_affichage=new QLabel("<b>Affichage Parametres:</b>",win);

	cb_display_keyspoints=new QCheckBox("Affichage des points clefs",win);
	cb_display_links=new QCheckBox("Affichage des liens",win);	

	view_selector=new QViewSelector();

	mode=new QComboBox(win);
	mode->addItem("Kalman XY");
	mode->addItem("Kalman XYWH"); 
	
	forceFirstPosition=new QPushButton("Forcer la premiere detection",win);
	
	sb_precision=new QSpinBox(win);
	sb_precision->setRange(1,128);
	sb_precision->setPrefix("Precision=");
	sb_precision->setValue(32);

	sb_marges=new QSpinBox(win);
	sb_marges->setRange(1,32);
	sb_marges->setPrefix("Marges=");
	sb_marges->setValue(5);
	sb_marges->setEnabled(false);	

	sb_distance_max=new QDoubleSpinBox(win);
	sb_distance_max->setRange(0.0,100.0);
	sb_distance_max->setDecimals(4);
	sb_distance_max->setPrefix("Distance_max=");
	sb_distance_max->setSingleStep(0.001);
	sb_distance_max->setValue(0.001);

	layout->addWidget(view_selector);
	sub_layout->addWidget(l_flann);
	sub_layout->addWidget(sb_precision);
	sub_layout->addWidget(sb_distance_max);	
	sub_layout->addWidget(l_kalman);	
	sub_layout->addWidget(mode);
	sub_layout->addWidget(forceFirstPosition);
	sub_layout->addWidget(sb_marges);
	sub_layout->addWidget(l_affichage);
	sub_layout->addWidget(cb_display_keyspoints);
	sub_layout->addWidget(cb_display_links);
	

	layout->addLayout(sub_layout);
	connect(forceFirstPosition,SIGNAL(clicked()),this,SLOT(s_forceFirstPosition()));
	connect(cb_display_keyspoints,SIGNAL(stateChanged(int)),this,SLOT(s_displayKeysPoints(int)));
	connect(cb_display_links,SIGNAL(stateChanged(int)),this,SLOT(s_displayLinks(int)));

	isKFMode1Init=false;
	isKFMode2Init=false;
}
////////////////////////////////////////////////////////////////////////////////
void ModuleTracking::free()
{
	delete win;
}
////////////////////////////////////////////////////////////////////////////////
cv::Mat & ModuleTracking::process()
{
	
	//On crée la liste des liens avec un algorithme de type flann	
	cv::Mat_<int> links;
	cv::Mat_<float> dists;
	if(previousDescriptors.rows>0 && previousDescriptors.cols>0 &&
		 currentDescriptors.rows>0 && currentDescriptors.cols>0)
	{
		getFLANN(previousDescriptors,
						 currentDescriptors,
						 links,dists,sb_precision->value());
	}
	
	
	
	float dmoy[2]={0,0};		  //vecteur deplacement moyen
	int n=0;									//nombre de liens
	int maxx=0,minx=100000,maxy=0,miny=100000;
	//////////////////////////////////////////////////////////////////////////////
	//Algorithme de Kalman
	//////////////////////////////////////////////////////////////////////////////
	
	//Calcul du  déplacement moyen a partir des liens pertinants
	for(int i=0;i<links.rows;i++)
	{
		if(dists(i)>=sb_distance_max->value() || links(i)>=previousKeysPoints.rows)
		{
			links(i)=-1;		
		}
		else
		{
			float dx=currentKeysPoints(i,0)-previousKeysPoints(links(i),0);
			float dy=currentKeysPoints(i,1)-previousKeysPoints(links(i),1);
			if(maxx<currentKeysPoints(i,0))maxx=currentKeysPoints(i,0);
			if(maxy<currentKeysPoints(i,1))maxy=currentKeysPoints(i,1);
			if(minx>currentKeysPoints(i,0))maxx=currentKeysPoints(i,0);
			if(miny>currentKeysPoints(i,1))maxy=currentKeysPoints(i,1);
						
			n++;
			dmoy[0]+=dx;
			dmoy[1]+=dy;
		}
	}
	
	//Si des liens existent on essaye de prédire l'emplacement de la box suivante
	
	if(isKFMode1Init  && mode->currentIndex()==0)
	{
		if(n>0)
		{	
			//Calcul de l'etat du systeme mesuré
		
			mesured_state(0,0)+=dmoy[0]/n;//x
			mesured_state(1,0)+=dmoy[1]/n;//y
			mesured_state(2,0)=dmoy[0]/n;
			mesured_state(3,0)=dmoy[1]/n;	

			//Correction et prédiction du modele	
			KF.correct(mesured_state);		
			predicted_state=KF.predict();
		

			//printf("Mesured_state: %f %f %f %f\n",mesured_state(0,0),mesured_state(1,0),mesured_state(2,0),mesured_state(3,0));
			//printf("Predicted_state: %f %f %f %f\n",predicted_state(0,0),predicted_state(1,0),predicted_state(2,0),predicted_state(3,0));		

			//On deplace notre box a l'endroit prédit
			view_selector->set_box_xy(predicted_state);	
			view_selector->setKeysPoints(currentKeysPoints,previousKeysPoints);
			view_selector->setLinks(links);
		}
		else if(isKFMode1Init)
		{
			view_selector->get_box_xy(mesured_state);
			KF.correct(mesured_state);		
			predicted_state=KF.predict();
		}
	}
	else if(isKFMode2Init && mode->currentIndex()==1)
	{
		if(n>0)
		{	
			//Calcul de l'etat du systeme mesuré

			mesured_state(0,0)+=dmoy[0]/n;//x
			mesured_state(1,0)+=dmoy[1]/n;//y
			mesured_state(2,0)=dmoy[0]/n;
			mesured_state(3,0)=dmoy[1]/n;	

			mesured_state(6,0)=mesured_state(4,0);
			mesured_state(7,0)=mesured_state(5,0);
			mesured_state(4,0)=maxx-minx;//w
			mesured_state(5,0)=maxy-miny;//h
			mesured_state(6,0)-=mesured_state(4,0);
			mesured_state(7,0)-=mesured_state(5,0);	

			//Correction et prédiction du modele	
			KF.correct(mesured_state);		
			predicted_state=KF.predict();


			//printf("Mesured_state: %f %f %f %f\n",mesured_state(0,0),mesured_state(1,0),mesured_state(2,0),mesured_state(3,0));
			//printf("Predicted_state: %f %f %f %f\n",predicted_state(0,0),predicted_state(1,0),predicted_state(2,0),predicted_state(3,0));		

			//On deplace notre box a l'endroit prédit
			view_selector->set_box_xywh(predicted_state);	
			view_selector->setKeysPoints(currentKeysPoints,previousKeysPoints);
			view_selector->setLinks(links);
		}
		else if(isKFMode2Init)
		{
			view_selector->get_box_xywh(mesured_state);
			KF.correct(mesured_state);		
			predicted_state=KF.predict();
		}
	}

	//on renvoie le nouveau mask de suivis 
	return view_selector->getMask();
}
////////////////////////////////////////////////////////////////////////////////
void ModuleTracking::s_forceFirstPosition()
{
	//Initialisation du filtre de kalman	
	isKFMode1Init=false;
	isKFMode2Init=false;

	if(mode->currentIndex()==0)
	{
		//4 parametres : x y x' y' et pas de commande/controle
		KF.init(4,4,0);
		isKFMode1Init=true;

		//x y x' y'isKFMode1Init
		mesured_state=*(cv::Mat_<float>(4,1) <<0,0,0,0); 
		predicted_state=*(cv::Mat_<float>(4,1) <<0,0,0,0);

		view_selector->get_box_xy(predicted_state);
		view_selector->get_box_xy(mesured_state);

		KF.statePre=predicted_state;
		KF.statePost=predicted_state;

		KF.correct(mesured_state);		
		predicted_state=KF.predict();

		KF.transitionMatrix = *(cv::Mat_<float>(4,4) << 1,0,1,0,
																										0,1,0,1,
																										0,0,1,0,
																										0,0,0,1);
																										 
		//On mesure tous les parametres donc la matrice de mesure est la matrice identité
		cv::setIdentity(KF.measurementMatrix);
		//Le reste on initialise comme l'exemple
		cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));
		cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
		cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1));
		cv::randn(KF.statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));
	}
	else if(mode->currentIndex()==1)
	{
		//8 parametres : x y x' y' w h w' h' et pas de commande/controle
		KF.init(8,8,0);
		isKFMode2Init=true;

		//x y x' y' w h w' h'
		mesured_state=*(cv::Mat_<float>(8,1) <<0,0,0,0,0,0,0,0); 
		predicted_state=*(cv::Mat_<float>(8,1) <<0,0,0,0,0,0,0,0);

		view_selector->get_box_xywh(predicted_state);
		view_selector->get_box_xywh(mesured_state);

		KF.statePre=predicted_state;
		KF.statePost=predicted_state;

		KF.correct(mesured_state);		
		predicted_state=KF.predict();

		KF.transitionMatrix = *(cv::Mat_<float>(8,8) << 1,0,1,0,0,0,0,0,
																										0,1,0,1,0,0,0,0,
																										0,0,1,0,0,0,0,0,
																										0,0,0,1,0,0,0,0,
																										0,0,0,0,1,0,1,0,
																										0,0,0,0,0,1,0,1,
																										0,0,0,0,0,0,1,0,
																										0,0,0,0,0,0,0,1);
																										 
		//On mesure tous les parametres donc la matrice de mesure est la matrice identité
		cv::setIdentity(KF.measurementMatrix);
		//Le reste on initialise comme l'exemple
		cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-5));
		cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
		cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1));
		cv::randn(KF.statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));
	}

	parent->forceOutput(view_selector->getMask());
}
////////////////////////////////////////////////////////////////////////////////
void ModuleTracking::setInput(int n,const cv::Mat & m)
{
	if(n==0)
	{	
		currentFrame=m.clone();
		view_selector->setImage(currentFrame);
	}
	else if(n==1)
	{
		previousKeysPoints=currentKeysPoints.clone();
		currentKeysPoints=m.clone();
	}
	else if(n==2)
	{
		previousDescriptors=currentDescriptors.clone();
		currentDescriptors=m.clone();
	}
	
}
////////////////////////////////////////////////////////////////////////////////
QWidget* ModuleTracking::widget()
{
	return win;
}
////////////////////////////////////////////////////////////////////////////////
QString ModuleTracking::getName()
{
	return tr("Tracking");
}
////////////////////////////////////////////////////////////////////////////////
int ModuleTracking::nbInputs()
{
	return 3;
}
////////////////////////////////////////////////////////////////////////////////
QString ModuleTracking::getParams()
{
  return tr("%n;","",sb_precision->value())+QString::number(sb_distance_max->value())+tr(";")+tr("%n;","",cb_display_keyspoints->isChecked())
					+tr("%n","",cb_display_links->isChecked());
}
////////////////////////////////////////////////////////////////////////////////
bool ModuleTracking::setParams(QString params)
{
  QStringList list_params=params.split(";");
		
	if(list_params.size()==4)
	{
		sb_precision->setValue(list_params[0].toInt());
		sb_distance_max->setValue(list_params[1].toFloat());
		cb_display_keyspoints->setChecked(list_params[2].toInt());
		cb_display_links->setChecked(list_params[3].toInt());

		return true;
	}
	else
	{
		return false;
	}
}
////////////////////////////////////////////////////////////////////////////////
QCVModuleInterface * ModuleTracking::clone()
{
	return new ModuleTracking;
}
////////////////////////////////////////////////////////////////////////////////
QString ModuleTracking::help(int id)
{
	if(id==-2)
	{
		return tr("Genere un mask de tracking");
	}
	else if(id==-1)
	{
		return tr("image");
	}
	else if(id==0)
	{
		return tr("image");
	}
	else if(id==1)
	{
		return tr("keyspoints");
	}
	else if(id==2)
	{
		return tr("descripteurs");
	}
	else
	{
		return tr("erreur");
	}
}
////////////////////////////////////////////////////////////////////////////////
void ModuleTracking::s_displayLinks(int state)
{
	view_selector->displayLinks(state);
}
////////////////////////////////////////////////////////////////////////////////
void ModuleTracking::s_displayKeysPoints(int state)
{
	view_selector->displayKeys(state);
}
////////////////////////////////////////////////////////////////////////////////
Q_EXPORT_PLUGIN2(ModuleTracking_plugin,ModuleTracking);  
